//
// Created by 张荣开 on 25-6-1.
//

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "controlcan.h"
#include "cpp_can/msg/car_state.hpp"
#include "geometry_msgs/msg/twist.hpp"  // 新增头文件，用于cmd_vel话题

using namespace std;
using namespace std::chrono_literals;
VCI_BOARD_INFO pInfo; // 用来获取设备信息

//设置刹车的CAN数据发送函数
//刹车力度设置函数  0x00-0x05  0x05为急刹 
//默认为 0x00
void SetBrek(int brek) 
{
    // 需要发送的帧，结构体设置
    VCI_CAN_OBJ send[1];
    send[0].ID = 0x00000001; // 根据底盘can协议，发送时候can的ID为0x01
    send[0].SendType = 0;
    send[0].RemoteFlag = 0;
    send[0].ExternFlag = 0;
    send[0].DataLen = 8;

    // 要写入的数据
    send[0].Data[0] = 0x01;
    send[0].Data[1] = 0x04;
    send[0].Data[2] = brek;
    send[0].Data[3] = 0x00;
    send[0].Data[4] = 0x00;
    send[0].Data[5] = 0x00;
    send[0].Data[6] = 0x00;
    send[0].Data[7] = 0x00;

    // 打印发送的数据
    printf("发送CAN数据 - ID: 0x%08X, 数据: ", send[0].ID);
    for(int i = 0; i < send[0].DataLen; i++) {
        printf("0x%02X ", send[0].Data[i]);
    }
    printf("\n");

    // 写入数据
    if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
    {
        printf("CAN数据发送成功\n");
    }
    else
    {
        printf("CAN数据发送失败\n");
    }
} 

//阿克慢前后轮距离
#define WHELLBASE 0.785

//发送速度和角度到AKM的CAN数据发送函数
void SendSpeedToAKM(float lineSpeed, float angle_speed)
{
    float R = 0;
    float angle_rad = 0;  // 弧度制角度
    float angle_deg = 0;  // 角度制角度

    if (lineSpeed != 0 && angle_speed != 0)
    {
        R = lineSpeed / angle_speed;
        angle_rad = atan(WHELLBASE / R);
        
        // 转换为角度制（弧度->度）
        angle_deg = angle_rad * 180.0f / M_PI;

        
        // 限制最大角度为±30°
        if (angle_deg > 30.0f) angle_deg = 30.0f;
        if (angle_deg < -30.0f) angle_deg = -30.0f;
    }

    // 单位转换
    int LineSpeed = static_cast<int>(lineSpeed * 1000);  // m/s -> mm/s
    int Angle = static_cast<int>(angle_deg * 100);       // 角度放大100倍（如21°→2100）
    
    // CAN帧设置
    VCI_CAN_OBJ send[1];
    send[0].ID = 0x00000001;
    send[0].SendType = 0;
    send[0].RemoteFlag = 0;
    send[0].ExternFlag = 0;
    send[0].DataLen = 8;

    // 数据填充（注意：负数的高低位处理由系统自动完成补码转换）
    send[0].Data[0] = 0x01;  // 功能码
    send[0].Data[1] = 0x01;  // 子功能码
    send[0].Data[2] = LineSpeed & 0xFF;                 // 线速度低8位
    send[0].Data[3] = (LineSpeed >> 8) & 0xFF;          // 线速度高8位
    send[0].Data[4] = static_cast<uint8_t>(Angle & 0xFF);        // 角度低8位
    send[0].Data[5] = static_cast<uint8_t>((Angle >> 8) & 0xFF); // 角度高8位
    send[0].Data[6] = 0x00;
    send[0].Data[7] = 0x00;
    
    // 打印调试信息
    printf("发送CAN数据 - ID: 0x%08X, 数据: ", send[0].ID);
    for(int i = 0; i < send[0].DataLen; i++) {
        printf("0x%02X ", send[0].Data[i]);
    }
    printf("\n");
    // 打印线速度（原始值 + CAN转换值）
    cout << "原始线速度: " << fixed << setprecision(3) << lineSpeed << " m/s" << endl;
    cout << "CAN线速度: " << LineSpeed << " mm/s" << endl;

    // 打印转向角度（计算后角度值 + CAN转换值）
    cout << "转向角度: " << fixed << setprecision(1) << angle_deg << " °" << endl;
    cout << "CAN转向角度: " << Angle << " (度×100)" << endl;



    // 发送数据
    if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
    {
       printf("数据发送成功\n");
    }
    else
    {
       printf("数据发送失败\n");
    }
}



/*
mode: 0:常亮 1:闪烁 2:呼吸
light:亮度(0-100)
delay:间隔时间(1-255)对应时间(0.1-25.5s)
R:(0-255)颜色值
G:(0-255)
B:(0-255)
*/
void SetLed(unsigned char mode,unsigned char light,unsigned char delay,unsigned char R,unsigned char G,unsigned char B) 
{
     //需要发送的帧，结构体设置
    VCI_CAN_OBJ send[1];
    send[0].ID=0x00000001;//根据底盘can协议，发送时候can的ID为0x01
    send[0].SendType=0;
    send[0].RemoteFlag=0;
    send[0].ExternFlag=0;
    send[0].DataLen=8;

    //要写入的数据
    send[0].Data[0] = 0x01;
    send[0].Data[1] = 0x05;
    send[0].Data[2] = mode;
    send[0].Data[3] = light;
    send[0].Data[4] = delay;
    send[0].Data[5] = R;
    send[0].Data[6] = G;
    send[0].Data[7] = B;

    //写入数据
    if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
    {
            printf(">>设置灯光成功n"); // 打开设备成功
    }
}

//档位设置函数0：低速档，1：中速档，2：高速档，默认低速档
void SetGear(int gear)
{
     //需要发送的帧，结构体设置
    VCI_CAN_OBJ send[1];
    send[0].ID=0x00000001;//根据底盘can协议，发送时候can的ID为0x01
    send[0].SendType=0;
    send[0].RemoteFlag=0;
    send[0].ExternFlag=0;
    send[0].DataLen=8;

    //要写入的数据
    send[0].Data[0] = 0x01;
    send[0].Data[1] = 0x03;
    send[0].Data[2] = gear;
    send[0].Data[3] = 0x00;
    send[0].Data[4] = 0x00;
    send[0].Data[5] = 0x00;
    send[0].Data[6] = 0x00;
    send[0].Data[7] = 0x00;

    //写入数据
    if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
    {
            printf("设置档位成功\n"); // 打开设备成功       
    }
}

// 自定义节点类 - 整合后的CAN节点（发送+接收）
class CANSenderNode : public rclcpp::Node
{
public:
    CANSenderNode() : Node("can_sender_node"), current_state_(0), start_time_(this->now())
    {
        // 初始化CAN设备
        device_type = VCI_USBCAN2; // 设备类型修改
        device_index = 0;          // 设备的索引
        can_index = 0;             // CAN控制器的索引 CAN1为0，CAN2为1
        
        int num = VCI_OpenDevice(VCI_USBCAN2, 0, 0);
        if(num == 0 || num == 1) // 打开设备
        {
            printf(">>open deivce success!\n"); // 打开设备成功
        }
        else
        {
            printf(">>open deivce error %d!\n", num);
            exit(1);
        }
        
        if(VCI_ReadBoardInfo(VCI_USBCAN2, 0, &pInfo) == 1) // 读取设备序列号、版本等信息
        {
            printf(">>Get VCI_ReadBoardInfo success!\n");
        }
        else
        {
            printf(">>Get VCI_ReadBoardInfo error!\n");
            exit(1);
        }

        VCI_INIT_CONFIG config;
        config.AccCode = 0;
        config.AccMask = 0xFFFFFFFF; // FFFFFFFF全部接收
        config.Filter = 2; // 接收所有帧  2-只接受标准帧  3-只接受扩展帧
        config.Timing0 = 0x00; /*波特率500 Kbps  0x00  0x1C*/
        config.Timing1 = 0x1C;
        config.Mode = 0; // 正常模式
        

        if (VCI_InitCAN(device_type, device_index, can_index, &config) != STATUS_OK)
        {
            RCLCPP_FATAL(this->get_logger(), "------初始化CAN参数失败------");
            VCI_CloseDevice(device_type, device_index);
            return;
        }
        
        if(VCI_StartCAN(VCI_USBCAN2, 0, 0) != 1)
        {
            printf(">>Start CAN1 error\n");
            VCI_CloseDevice(VCI_USBCAN2, 0);
        }

        // 发送一次初始速度
        SendSpeedToAKM(0.0, 0.0);
        //设为低速档
        SetGear(0);
        //设刹车为0
        SetBrek(0);
        
        // 创建cmd_vel订阅者
        cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
            "cmd_vel", 
            50, 
            std::bind(&CANSenderNode::cmd_velCallback, this, std::placeholders::_1)
        );

        // 创建car_state发布者
        car_state_pub_ = this->create_publisher<cpp_can::msg::CarState>(
            "car_state",
            20
        );

        // 创建定时器，每10毫秒(0.01秒)调用一次发送和读取函数
        timer_ = this->create_wall_timer(
            10ms, 
            std::bind(&CANSenderNode::timer_callback, this)
        );
    }

    //速度控制回调
    void cmd_velCallback(const geometry_msgs::msg::Twist &twist_aux)
    {
        SendSpeedToAKM(twist_aux.linear.x, twist_aux.angular.z);
    }

    //读取小车状态
    void ReadCarState(void)
    {
        //需要读取的帧，结构体设置
        VCI_CAN_OBJ rev[2500];
        rev[0].SendType=0;
        rev[0].RemoteFlag=0;
        rev[0].ExternFlag=0;
        rev[0].DataLen=8;
        
        //读取数据
        auto re_size = VCI_Receive(VCI_USBCAN2, 0, 0, rev, 2500, 0);
        auto msg = cpp_can::msg::CarState();
        
        if(re_size > 0)
        {
            for(unsigned int i=0;i< re_size;i++)
            {
                if((rev[i].ID == 0x10)&&(rev[i].Data[0] == 0x02) && (rev[i].Data[1] == 0x02))//can发送02代表接收的数据
                {
                    back_wheel_speed    = (rev[i].Data[3] << 8) | rev[i].Data[2];//上报的后轮速度
                    turn_angle          = (rev[i].Data[5] << 8) | rev[i].Data[4];//上报的转向角度
                    battery_level       = rev[i].Data[6];//上报的电量百分比
                    error_flag          = rev[i].Data[7];//上报的错误状态
                    
                    msg.back_wheel_speed = back_wheel_speed;
                    msg.turn_angle = turn_angle/100.00*M_PI/180;
                    msg.battery_level = battery_level;
                    msg.error_flag = error_flag;

                    cout << "后轮速度: " << back_wheel_speed << " mm/s" << endl;
                    cout << "转向弧度: " << turn_angle/100.00*M_PI/180 << endl;
                    cout << "错误状态: " << error_flag << endl; 
                    cout << "电量百分比: " << battery_level << endl;
                    cout << "------------------------------------------------------"<< endl;

                    car_state_pub_->publish(msg);
                }
                else if((rev[i].ID == 0x11)&&(rev[i].Data[0] == 0x02) && (rev[i].Data[1] == 0x02))//can发送02代表接收的数据
                {
                    left_speed    = (rev[i].Data[3] << 8) | rev[i].Data[2];//上报的后轮速度
                    right_speed   = (rev[i].Data[5] << 8) | rev[i].Data[4];//上报的转向角度
                    
                    msg.left_speed  = left_speed;        //上报的后左轮速度
                    msg.right_speed = right_speed;       //上报的后右轮速度

                    cout << "后左轮速度: " << left_speed << endl; 
                    cout << "后右轮速度: " << right_speed << endl;

                    cout << "------------------------------------------------------"<< endl;
                    
                    car_state_pub_->publish(msg);
                }
                else if(rev[i].ID == 0x12)//can发送02代表接收的数据
                {
                    msg.front_current   = (short)((rev[i].Data[1] << 8) | rev[i].Data[0]);//上报的后轮速度
                    msg.left_current    = (short)((rev[i].Data[3] << 8) | rev[i].Data[2]);//上报的转向角度
                    msg.right_current   = (short)((rev[i].Data[5] << 8) | rev[i].Data[4]);//上报的转向角度
                    
                    cout << "转向电机电流: " << msg.front_current << endl; 
                    cout << "左后电机电流: " << msg.left_current  << endl;
                    cout << "右后电机电流: " << msg.right_current << endl;

                    cout << "------------------------------------------------------"<< endl;
                    
                    car_state_pub_->publish(msg);
                }
            }
        }
    }
    void timer_callback()
    {
        ReadCarState();
    }


private:

    // 声明为类的成员变量
    int current_state_;                  // 状态变量
    rclcpp::Time start_time_;            // 时间变量

    rclcpp::TimerBase::SharedPtr timer_; // 定时器
    
    // cmd_vel订阅者
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
    rclcpp::Publisher<cpp_can::msg::CarState>::SharedPtr car_state_pub_; // 车辆状态发布者

    DWORD device_type;
    DWORD device_index;
    DWORD can_index;

    short back_wheel_speed = 0.0;   //上报的后轮速度
    short turn_angle = 0.0;         //上报的转向角度
    short battery_level = 0.0;      //上报的电量百分比
    short error_flag = 0.0;         //上报的错误状态
    short left_speed = 0.0;         //上报的后左轮速度
    short right_speed = 0.0;        //上报的后右轮速度
    
};

int main(int argc, char const *argv[])
{
    // 初始化ROS2客户端
    rclcpp::init(argc, argv);
    // 调用spin函数，并传入节点对象指针
    rclcpp::spin(std::make_shared<CANSenderNode>());
    
    // 资源释放
    VCI_CloseDevice(VCI_USBCAN2, 0);
    rclcpp::shutdown();
    
    return 0;
}
